#include "hand_drv/motor_control.hpp"

MotorController::MotorController(ros::NodeHandle& nh) : nh_(nh), position_(0.0), is_enabled_(false) {
   


   re_loopThread  = std::thread(&MotorController::can_recive, this); 


    init_drive();
    // 创建publisher，发布到名为'motor_position'的主题
    
    position_pub_ = nh_.advertise<std_msgs::Int32MultiArray>("motor_position", 10); //数组长度为4
    // 创建service server
    set_bool_server_ = nh_.advertiseService("set_motor_state", &MotorController::handleSetBool, this);
    set_position_server_ = nh_.advertiseService("set_motor_position", &MotorController::handleSetMotorPosition, this);
// usleep(1000*1000);
std::cout<<"发送数据："<<std::endl;
motor_init();
    // std::cout<<"aaa:   "<<start_CANOpen_cmd[0]<<std::endl;
    // usleep(10000*1000);
    open_hand();

}

void MotorController::start() {
    ros::Rate loop_rate(1); // 1 Hz
    while (ros::ok()) {
        //std::cout<<"+++++++++++++++++++++++++"<<std::endl;
     
        if (is_enabled_) {
            publishPosition();
        }
        ros::spinOnce();
        loop_rate.sleep();
    }
}

bool MotorController::handleSetBool(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) {
    is_enabled_ = req.data;
    if (is_enabled_) {
        ROS_INFO("Received command to turn ON the motor.");
        res.success = true;
        res.message = "Motor turned ON";
    } else {
        ROS_INFO("Received command to turn OFF the motor.");
        res.success = true;
        res.message = "Motor turned OFF";
    }
    return true;
}

bool MotorController::handleSetMotorPosition(hand_drv::SetMotorPosition::Request &req, hand_drv::SetMotorPosition::Response &res) {
    position_ = req.position;


    ROS_INFO("Received command to set motor position to: %f", position_);
    res.success = true;
    res.message = "Motor position set successfully";
    return true;
}

void MotorController::publishPosition() {
    std_msgs::Float64 msg;
    msg.data = position_;
    ROS_INFO("Publishing Motor Position: %f", msg.data);
    position_pub_.publish(msg);
}

int MotorController::send_data(int ID,  const int data[],int length) //CAN发送函数
{
      VCI_CAN_OBJ send[1];
    int i=0;
	send[0].ID=ID;//0x00007FFE
	send[0].SendType=0; //0时为正常发送（发送失败会自动重发，重发时间为4秒，4秒内没有发出则取消）
	                    //1时为单次发送（只发送一次，发送失败不会自动重发，总线只产生一帧数据）
	send[0].RemoteFlag=0;//数据帧0 远程帧1 
	send[0].ExternFlag=0; //扩展为1 标准为0
	send[0].DataLen=length; //发送数据长度

	for(i = 0; i < (send[0].DataLen); i++)
	{
		send[0].Data[i] = data[i];//发送数据赋值
        printf(" %02X",send[0].Data[i]);
	}
    printf("\n");
   
    usleep(10*1000);
    if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1)
    {
        return 1;
    }
    else
    {
       std::cout<<"+++++++++++++++++++++++++"<<std::endl;
        return 0;
    }
}
int  MotorController::init_drive()
{

        VCI_INIT_CONFIG config;
        config.AccCode=0;
        config.AccMask=0xFFFFFFFF;
        config.Filter=1;//接收所有帧
        config.Timing0=0x00;/*波特率125 Kbps  0x03  0x1C*/
        config.Timing1=0x1C;//波特率设置
        config.Mode=0;//正常模式
        //CAN口初始化
        if(VCI_OpenDevice(VCI_USBCAN2,0,0)!=1)//打开设备
        {
            printf(">>open deivce success!\n");//打开设备成功
        }else
        {
            printf(">>open deivce error!\n");
        }	
        //开启CAN1通信
        if(VCI_InitCAN(VCI_USBCAN2,0,0,&config)!=1)
        {
            printf(">>Init CAN1 error\n");
            VCI_CloseDevice(VCI_USBCAN2,0);
        }
        if(VCI_StartCAN(VCI_USBCAN2,0,0)!=1)
        {
            printf(">>Start CAN1 error\n");
            VCI_CloseDevice(VCI_USBCAN2,0);
        }
        //CAN2初始化
        if(VCI_InitCAN(VCI_USBCAN2,0,1,&config)!=1)
        {
            printf(">>Init can2 error\n");
            VCI_CloseDevice(VCI_USBCAN2,0);

        }
        if(VCI_StartCAN(VCI_USBCAN2,0,1)!=1)
        {
            printf(">>Start can2 error\n");
            VCI_CloseDevice(VCI_USBCAN2,0);
        }
        return 0;
    
}

void MotorController::can_recive()//CAN口接收函数
{
     int reclen=0;
	VCI_CAN_OBJ rec[3000];//接收缓存，设为3000为佳。
     uint8_t rxbuf[6]={0};
	int i,j;
    int ind=0;
    float dis_data;
    int  count=0;
   
        std_msgs::Int32MultiArray msg;
        msg.layout.dim.push_back(std_msgs::MultiArrayDimension());
        msg.layout.dim[0].size = 4;
        msg.layout.dim[0].stride = 4;
        msg.layout.dim[0].label = "array_size";
        msg.data.resize(4);
        while (true)
         {
            // std::cout<<"aaa"<<start_CANOpen_cmd[1]<<std::endl;
               usleep(1000*1000);
               

            if((reclen=VCI_Receive(VCI_USBCAN2,0,ind,rec,3000,100))>0)
                {

                   if (rec[j].ID==0x0281)
                   {
                         	printf(" %02X", rec[j].Data[2]);
                           	printf(" %02X", rec[j].Data[3]);
                        	printf(" %02X", rec[j].Data[4]);
                           	printf(" %02X", rec[j].Data[5]);
                               				printf("\n");
                        msg.data[0] =(rec[j].Data[2] + rec[j].Data[3]<<8+rec[j].Data[4]<<16+rec[j].Data[5]<<24)/116507;
                        std::cout<<"电机1号位置："<<msg.data[0] <<std::endl;
                   }


                     if (rec[j].ID==0x0181)
                   {
                         	printf(" %02X", rec[j].Data[2]);
                           	printf(" %02X", rec[j].Data[3]);
                        	printf(" %02X", rec[j].Data[4]);
                           	printf(" %02X", rec[j].Data[5]);
                               				printf("\n");
                        msg.data[0] =(rec[j].Data[2] + rec[j].Data[3]<<8+rec[j].Data[4]<<16+rec[j].Data[5]<<24)/116507;
                        std::cout<<"=====电机1号位置："<<msg.data[0] <<std::endl;
                   }
                if (rec[j].ID==0x0182)
                   {
                        msg.data[1] =(rec[j].Data[2] + rec[j].Data[3]<<8+rec[j].Data[4]<<16+rec[j].Data[5]<<24)/116507;
                   }
                if (rec[j].ID==0x0183)
                   {
                        msg.data[2] =(rec[j].Data[2] + rec[j].Data[3]<<8+rec[j].Data[4]<<16+rec[j].Data[5]<<24)/116507;
                   }
                    if (rec[j].ID==0x0184)
                   {
                        msg.data[3] =(rec[j].Data[2] + rec[j].Data[3]<<8+rec[j].Data[4]<<16+rec[j].Data[5]<<24)/116507;
                   }
                   
                    // position_pub_.publish(msg);
    	// for(j=0;j<reclen;j++)
		// 	{
		// 		printf("Index:%04d  ",count);count++;//序号递增
		// 		printf("CAN%d RX ID:0x%08X", ind+1, rec[j].ID);//ID
		// 		if(rec[j].ExternFlag==0) printf(" Standard ");//帧格式：标准帧
		// 		if(rec[j].ExternFlag==1) printf(" Extend   ");//帧格式：扩展帧
		// 		if(rec[j].RemoteFlag==0) printf(" Data   ");//帧类型：数据帧
		// 		if(rec[j].RemoteFlag==1) printf(" Remote ");//帧类型：远程帧
		// 		printf("DLC:0x%02X",rec[j].DataLen);//帧长度
		// 		printf(" data:0x");	//数据
		// 		for(i = 0; i < rec[j].DataLen; i++)
		// 		{
		// 			printf(" %02X", rec[j].Data[i]);
		// 		}
		// 		printf(" TimeStamp:0x%08X",rec[j].TimeStamp);//时间标识。
		// 		printf("\n");
		// 	}
        //     	ind=!ind;//变换通道号，以便下次读取另一通道，交替读取。	
          }
         }
         }

 int MotorController::send_data_sigle(int ID,  const int data) //CAN发送函数
{
      VCI_CAN_OBJ send[1];
    int i=0;
	send[0].ID=ID;//0x00007FFE
	send[0].SendType=0; //0时为正常发送（发送失败会自动重发，重发时间为4秒，4秒内没有发出则取消）
	                    //1时为单次发送（只发送一次，发送失败不会自动重发，总线只产生一帧数据）
	send[0].RemoteFlag=0;//数据帧0 远程帧1 
	send[0].ExternFlag=0; //扩展为1 标准为0
	send[0].DataLen=1; //发送数据长度
  

  	send[0].Data[0] = data;//发送数据赋值
     printf("single : %02X",send[0].Data[0] );

    printf("\n");
    usleep(10*1000);
    if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1)
    {
        return 1;
    }
    else
    {
      //  std::cout<<"+++++++++++++++++++++++++"<<std::endl;
        return 0;
    }
}


        int MotorController::motor_init()//初始化电机主要是开启CAN 和电机模式
        {

            //开启电机CANOpen
            for (size_t i = 0; i < 4; i++)
            {
                    printf("   %02X",SET_MODEL[i]);
                   printf("\n");
                 start_CANOpen_cmd[1]=i+1;
                 printf("CANOPEN:::");
                  printf(" %02X",start_CANOpen_cmd[0]);
                   printf("   %02X",start_CANOpen_cmd[1]);
                   printf("\n");
                send_data(START_CANOPEN[i], start_CANOpen_cmd,(sizeof(start_CANOpen_cmd) / sizeof(start_CANOpen_cmd[0])));
            }
          //设置电机为位置模式
           for (size_t i = 0; i < 4; i++)
            {
                    printf("   %02X",SET_MODEL[i]);
                   printf("\n");
                send_data_sigle(SET_MODEL[i], set_model_data);
            }
         //开启位置模式
           for (size_t i = 0; i < 4; i++)
            {
                control_data[0]=0x06;
                   printf("   %02X",SET_Contrlo[i]);
                   printf("\n");
                send_data(SET_Contrlo[i], control_data,(sizeof(control_data) / sizeof(control_data[0])));
                control_data[0]=0x07;
                send_data(SET_Contrlo[i], control_data,(sizeof(control_data) / sizeof(control_data[0])));
                control_data[0]=0x0F;
                send_data(SET_Contrlo[i], control_data,(sizeof(control_data) / sizeof(control_data[0])));
            }

            return 1;

            
        }
        int MotorController::set_posetion(int ID,int pos,int spd)
        {
           pos=pos*116507;
           std::cout<<"目标位置："<<pos<<std::endl;
            pos_data[0]=pos & 0x000000FF;
            pos_data[1]=(pos & 0x0000FF00) >>8;
            pos_data[2]=(pos & 0x00FF0000) >>16;
            pos_data[3]=(pos & 0xFF000000) >>24;
              pos_data[4]=spd & 0x00FF;
            pos_data[5]=(spd & 0xFF00)>>8;
          

            send_data(P_ID[ID], pos_data,(sizeof(pos_data) / sizeof(pos_data[0])));

            int data_temp[2]={0x1F,0x00};
            send_data(SET_Contrlo[ID], data_temp,(sizeof(data_temp) / sizeof(data_temp[0])));
        }

        int MotorController::open_hand() //开启手
        {
        //    set_posetion(0,-90,50000);
             set_posetion(0,30,10000);
        }
        int MotorController::close_hand()//关闭手
        {
           set_posetion(0,30,10000);
        }







